from Maix import MIC_ARRAY as mic
import lcd,time

from machine import Timer,PWM
import time
import math
import image
from fpioa_manager import fm
from Maix import GPIO

io_led_blue = 15
fm.register(io_led_blue, fm.fpioa.GPIO0)

led_b=GPIO(GPIO.GPIO0, GPIO.OUT)
led_b.value(1)

i_times=0 #计数
sum_of_angle=0 #总数
sum_of_intesity=0 #总数
angle_result=0 #均值结果
intesity_result=0
error_too_large_times=0 #误差过大次数
error_unstable_flag=0 #不稳定状态
angle_filter_4in1=0#四次滤波
temp_array_of_angle= [0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20]#产生二十个数的数组
temp_array_of_intesity= [0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20]#产生二十个数的数组
array16=[0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15]
row = 16
col = 16
array256= [[0 for i in range(col)] for j in range(row)]#产生16*16的数组
angle_of_steering=0
angle_of_steering_now=0

img=image.Image()
#PWM通过定时器配置，接到IO17引脚
tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
S1 = PWM(tim, freq=200, duty=0, pin=17)

def Servo(servo,angle):
    S1.duty((angle)/180*35+32)


def steering_angle_change(tim1):
    global angle_of_steering_now
    error_of_steering=angle_of_steering-angle_of_steering_now

    if  error_of_steering > 0.2:
        angle_of_steering_now += 0.2
    elif error_of_steering < -0.2:
        angle_of_steering_now -= 0.2
    else:
        angle_of_steering_now=angle_of_steering
 #   print(error_of_steering)
    Servo(S1,angle_of_steering_now)

tim1 = Timer(Timer.TIMER1, Timer.CHANNEL0, mode=Timer.MODE_PERIODIC, period=10, callback=steering_angle_change)


#mic.init(i2s_d0=34, i2s_d1=8, i2s_d2=33, i2s_d3=9, i2s_ws=32, i2s_sclk=10,\
 #           sk9822_dat=7, sk9822_clk=35)#可自定义配置 IO
mic.init()#默认配置
lcd.init()
#函数自定义区域
def get_mic_dir():
    no_zero_times=0
    AngleX=0
    AngleY=0
    AngleR=0
    Angle=0
    AngleAddPi=0
    mic_list=[]
    imga = mic.get_map()    # 获取声音源分布图像
    b = mic.get_dir(imga)   # 计算、获取声源方向
    for i in range(len(b)):
        if b[i]>=2:
            AngleX+= b[i]*math.sin(i*math.pi/6)
            AngleY+= b[i]*math.cos(i*math.pi/6)

    AngleX=round(AngleX,6) #计算坐标转换值
    AngleY=round(AngleY,6)
    for i in range(0,16,1):
        for j in range(0,6,1):
            array256[i][j]=imga.get_pixel(i,j)
            if array256[i][j]!=0:
                no_zero_times+=1

    if no_zero_times >4 :
        if AngleY<0:AngleAddPi=180
        if AngleX<0 and AngleY > 0:AngleAddPi=360
        if AngleX!=0 or AngleY!=0: #参数修正
            if AngleY==0:
                Angle=90 if AngleX>0 else 270 #填补X轴角度
            else:
                Angle=AngleAddPi+round(math.degrees(math.atan(AngleX/AngleY)),4) #计算角度
            AngleR=round(math.sqrt(AngleY*AngleY+AngleX*AngleX),4) #计算强度
            mic_list.append(AngleX)
            mic_list.append(AngleY)
            mic_list.append(AngleR)
            mic_list.append(Angle)
    a = mic.set_led(b,(0,0,255))# 配置 RGB LED 颜色值
    return mic_list #返回列表，X坐标，Y坐标，强度，角度

middle_area_x=25
middle_area_y=50
def draw_background_img():

    background_img=img
    background_img.draw_rectangle(middle_area_x-24, middle_area_y+50,25,50,lcd.GREEN,1)
    background_img.draw_rectangle(middle_area_x, middle_area_y,165,150,lcd.RED,1)
    background_img.draw_rectangle(middle_area_x+165, middle_area_y,25,150,lcd.BLUE,1)
    return background_img
#回调函数写入区域

tim1.start()
#一次定义区域
lcd.display(draw_background_img())

x=-30
squre_x=7
squre_y=75
angle_x=60
distance_x=275
string_jiaodu_zhongwen="angle:"
string_juli_zhongwen="distance:"
angle_of_steering=0;
time.sleep(3)
led_b.value(0)
#主循环区域
while True:
    sum_times=0
    sum_of_avg=0
    i=20

    while i>0:
       array_result= get_mic_dir()
       if array_result:
            angle_filter_4in1=angle_filter_4in1/2+array_result[3]/2
            temp_array_of_angle[i]=array_result[3]
            temp_array_of_intesity[i]=array_result[2]
            i -=1

     #  time.sleep_ms(200)
    temp_array_of_angle=sorted(temp_array_of_angle)
    temp_array_of_intesity=sorted(temp_array_of_intesity)
    for i in range(10):
        sum_of_angle += temp_array_of_angle[i+5]
        sum_of_intesity+=temp_array_of_intesity[i+5]

    angle_result = sum_of_angle / 10
    intesity_result=sum_of_intesity/10
    sum_of_intesity=0
    sum_of_angle=0
    print(angle_result)
 #   print(intesity_result)
    print(temp_array_of_angle)
    angle_real=180-angle_result-14
    if -40 > angle_real :
        angle_real=-40
    if angle_real > 40 :
        angle_real=40
    distance_x=275/math.cos(angle_real/180*math.pi)
    squre_y=250*math.tan(angle_real/180*math.pi)/300*150
    if -90 > squre_y :
        squre_y=-90
    if squre_y > 90 :
        squre_y=90
    angle_of_steering=angle_real
    led_b.value(1)
    print(angle_real)
    print(squre_y,"e")
    lcd.display(draw_background_img())
    string_jiaodu_with_number=string_jiaodu_zhongwen
    string_juli_with_number=string_juli_zhongwen
    lcd.draw_string(middle_area_x+200,85,string_jiaodu_with_number,lcd.RED,lcd.BLACK)
    lcd.draw_string(middle_area_x+200,115,str(angle_real),lcd.RED,lcd.BLACK)
    lcd.draw_string(middle_area_x+200,145,string_juli_with_number,lcd.RED,lcd.BLACK)
    lcd.draw_string(middle_area_x+200,175,str(distance_x),lcd.RED,lcd.BLACK)
    lcd.fill_rectangle(int(190+squre_x),int(125+squre_y), 8, 8, lcd.YELLOW)
    #time.sleep_ms(500)
    #-90度
